#ifndef MATH_UTILS_H
#define MATH_UTILS_H

#include <Eigen/Geometry> 
#include <iostream>
#include <vector>
using namespace std;

namespace math_utils {

static Eigen::Matrix<double, 2, 2> rotation(const double &theta) {
    Eigen::Matrix<double, 2, 2> R;
    R(0, 0) = std::cos(theta);
    R(1, 0) = std::sin(theta);
    R(1, 1) = R(0, 0);
    R(0, 1) = -R(1, 0);
    return R;
}

static double theta_normalize(const double &theta) {
    return std::atan2(std::sin(theta), std::cos(theta));
}

static double normalize_angle(double theta) {
  if (theta >= -M_PI && theta < M_PI)
    return theta;

  double multiplier = floor(theta / (2*M_PI));
  theta = theta - multiplier*2*M_PI;
  if (theta >= M_PI)
    theta -= 2*M_PI;
  if (theta < -M_PI)
    theta += 2*M_PI;

  return theta;
}

static Eigen::Matrix3d compute_pose(const Eigen::Vector3d &pose) {
    Eigen::Matrix3d target_pose = Eigen::Matrix3d::Identity();

    Eigen::Matrix<double, 2, 2> rotation = math_utils::rotation(pose[2]);

    target_pose.block<2, 2>(0,0) = rotation.block<2, 2>(0,0);
    target_pose.block<2, 1>(0,2) = pose.block<2, 1>(0,0);
    return target_pose;
}

// static Eigen::Vector3d compute_odometry_pose(const Eigen::Matrix3d &pose) {
//     Eigen::Matrix<double, 2, 2> rotation = math_utils::rotation(pose[2]);
//     Eigen::Vector3d target_pose;
//     target_pose[0] = pose(0,2);
//     target_pose[1] = pose(1,2);
//     target_pose[2] = std::atan2(pose(1,0),pose(0,0));
//     return target_pose;
// }

// static Eigen::Vector3d compute_delta_odometry(const Eigen::Vector3d &last_odometry, const Eigen::Vector3d &curr_odometry) {
//     Eigen::Matrix3d delta_pose = math_utils::compute_pose(last_odometry).inverse() * math_utils::compute_pose(curr_odometry);
//     Eigen::Vector3d delta_odom = math_utils::compute_odometry_pose(delta_pose);
//     return delta_odom;
// }

}
#endif
